###########################################################################
# Happy Hare supporting macros
#  Customization of loading and unload sequences
#
# These skeleton macros define all the callbacks made during filament loading
# or unloading. They can be used for the insertion of logic specific to your
# printer setup. The SAVE/RESTORE_GCODE_STATE wrapper pattern is precautionary
#
# The ordering of these macros is as follows (if any are not defined they
# are skipped):
#
# Unloading sequence...
#   _MMU_PRE_UNLOAD             Called before starting the unload
#     'form_tip_macro'          User defined macro for tip forming
#   _MMU_POST_FORM_TIP          Called immediately after tip forming
#     (_MMU_UNLOAD_SEQUENCE)    Advanced: Optionally called based on 'gcode_unload_sequence'
#   _MMU_POST_UNLOAD            Called after unload completes
#
# Loading sequence...
#   _MMU_PRE_LOAD               Called before starting the load
#     (_MMU_LOAD_SEQUENCE)      Advanced: Optionally called based on 'gcode_load_sequence'
#   _MMU_POST_LOAD              Called after the load is complete
#
# If changing a tool the unload sequence will be immediately followed by the load sequence
#
# Notes about built-in Happy Hare functionality (thus you don't need to worry about):
# 1. Happy Hare separately implements z-hop moves on toolchange (including EndlessSpool)
#    specified with the 'z_hop_height_toolchange' parameter as well as on errors
#    and print cancellation with 'z_hop_error' parameter.
# 2. Toolhead will be correctly placed prior to resuming the print although the logic
#    will only be expecting to correct z_hop height and will be slow if horizonal moves
#    are necessary
# 3. Pressure advance will automatically be restored after tip forming
# 4. M220 & M221 overrides will be retained after a toolchange
# 5. If configured, Spoolman will be notified of toolchange
# 6. Should an error occur causing a pause, the extruder temperature will be
#    saved and restored on MMU_UNLOCK or resume
#
# Leveraging the basic callbacks is usually sufficent for customization, however
# if you really want to do something unusual you can enable the gcode
# loading/unloading sequences by setting the following in 'mmu_parameters.cfg'
#
#   'gcode_load_sequence: 1'
#   'gcode_unload_sequence: 1'
#
# This is quite advanced and you will need to understand the Happy Hare state
# machine before embarking on changes. See doc
#


###########################################################################
# Variables controlling sequence macros are all set here
#
[gcode_macro _MMU_Variables]
variable_enable_park: 1			# Whether the parking move is enabled. Turn on after setting park_xy
variable_park_xy: 50, 50		# Coordinates of park position for toolchange
variable_park_z_hop: 1.0		# Additional Z_hop when toolchanging (working in and out of print)
variable_travel_speed: 166		# XY travel speed in mm/s
variable_lift_speed: 15			# Z travel speed in mm/s
variable_auto_home: 1			# Automatically home if necessary
variable_park_after_form_tip: 0		# Set to 1 if tip cutting at toolhead to delay move to park position

# -------------------------- Internal Don't Touch -------------------------
variable_saved_xyz: 0, 0, 0
variable_saved_pos: 0
variable_initial_park_z: 9999
gcode:					# Leave empty


###########################################################################
# Shared toolhead parking macro designed to position toolhead at a suitable
# parking or purging area
#   Z_HOP | float - override z-hop default
#
[gcode_macro _MMU_PARK]
description: Park toolhead safely away from print
gcode:
    {% set park_z_hop = params.Z_HOP|default(printer['gcode_macro _MMU_Variables'].park_z_hop)|float %}
    {% set enable_park = printer['gcode_macro _MMU_Variables'].enable_park|int %}
    {% set x, y = printer['gcode_macro _MMU_Variables'].park_xy|map('float') %}
    {% set initial_park_z = printer['gcode_macro _MMU_Variables'].initial_park_z|float %}
    {% set travel_speed = printer['gcode_macro _MMU_Variables'].travel_speed|float * 60 %}
    {% set lift_speed = printer['gcode_macro _MMU_Variables'].lift_speed|float * 60 %}
    {% set pos = printer.gcode_move.gcode_position %}
    {% set origin = printer.gcode_move.homing_origin %}
    {% set max = printer.toolhead.axis_maximum %}
    {% set z = [[pos.z, initial_park_z]|min + park_z_hop, max.z - origin.z]|min %}

    {% if enable_park %}
        {% if not 'xy' in printer.toolhead.homed_axes %}
            RESPOND MSG="Cannot park because XY not homed"
        {% else %}
            G90					# Absolute
            {% if park_z_hop > 0 %}
                {% if not 'z' in printer.toolhead.homed_axes %}
                    RESPOND MSG="Skipping z_hop because Z not homed"
                {% else %}
                    {% if initial_park_z >= 9999 %}
                        SET_GCODE_VARIABLE MACRO=_MMU_Variables VARIABLE=initial_park_z VALUE={pos.z}
                    {% endif %}
                    G1 Z{z} F{lift_speed}	# Z lift applied first
                {% endif %}
            {% endif %}
            G1 X{x} Y{y} F{travel_speed}	# Move to park position
        {% endif %}
    {% endif %}


###########################################################################
# Helper macro: save current toolhead position
#
[gcode_macro _MMU_SAVE_POSITION]
description: Record to toolhead position for return later
gcode:
    {% set saved_pos = printer['gcode_macro _MMU_Variables'].saved_pos %}
    {% set pos = printer.gcode_move.gcode_position %}
    {% set x, y, z = pos.x, pos.y, pos.z %}

    {% if not saved_pos and 'xyz' in printer.toolhead.homed_axes %}
        SET_GCODE_VARIABLE MACRO=_MMU_Variables VARIABLE=saved_xyz VALUE="{x}, {y}, {z}"
        SET_GCODE_VARIABLE MACRO=_MMU_Variables VARIABLE=saved_pos VALUE=1
    {% endif %}


###########################################################################
# Helper macro: restore previously saved position
#
[gcode_macro _MMU_RESTORE_POSITION]

gcode:
    {% set x,y,z = printer['gcode_macro _MMU_Variables'].saved_xyz|map('float') %}
    {% set saved_pos = printer['gcode_macro _MMU_Variables'].saved_pos %}
    {% set travel_speed = printer['gcode_macro _MMU_Variables'].travel_speed|float * 60 %}
    {% set lift_speed = printer['gcode_macro _MMU_Variables'].lift_speed|float * 60 %}
    {% set mmu_paused = printer.mmu.is_locked %}

    {% if saved_pos and not mmu_paused %}
        G90				# Absolute
        G1 X{x} Y{y} F{travel_speed}	# Restore X,Y position
        {% if 'z' in printer.toolhead.homed_axes %}
            G1 Z{z} F{lift_speed}	# Restore Z position as last step
        {% endif %}
    {% endif %}
    _MMU_CLEAR_POSITION


###########################################################################
# Helper macro: clear previously saved position
#
[gcode_macro _MMU_CLEAR_POSITION]
description: Clear previously recorded toolhead position and park z_hop
gcode:
    SET_GCODE_VARIABLE MACRO=_MMU_Variables VARIABLE=saved_pos VALUE=0
    SET_GCODE_VARIABLE MACRO=_MMU_Variables VARIABLE=initial_park_z VALUE=9999


###########################################################################
# Helper macro: restore previously saved position
#
[gcode_macro _MMU_AUTO_HOME]
description: Convenience auto homing primarily for testing
gcode:
    {% set auto_home = printer['gcode_macro _MMU_Variables'].auto_home|int %}

    {% if not 'xy' in printer.toolhead.homed_axes and auto_home %}
        RESPOND MSG="Automatically homing XY"
        G28 X Y
    {% endif %}


###########################################################################
# This occurs prior to unloading filament on a toolchange
#
[gcode_macro _MMU_PRE_UNLOAD]
description: Optional pre unload routine for filament change
gcode:
    {% set park_after_form_tip = printer['gcode_macro _MMU_Variables'].park_after_form_tip|int %}

    SAVE_GCODE_STATE NAME=MMU_PRE_UNLOAD_state

    # Typically you would move toolhead to a position where oozing is not
    # a problem when using standalone tip forming and perhaps do nothing in
    # toolhead tip cutting case
    _MMU_AUTO_HOME
    _MMU_SAVE_POSITION
    {% if not park_after_form_tip %}
        _MMU_PARK
    {% endif %}

    RESTORE_GCODE_STATE NAME=MMU_PRE_UNLOAD_state


###########################################################################
# This occurs immediately after the tip forming or cutting procedure
#
[gcode_macro _MMU_POST_FORM_TIP]
description: Optional post tip forming/cutting routing
gcode:
    {% set park_after_form_tip = printer['gcode_macro _MMU_Variables'].park_after_form_tip|int %}

    SAVE_GCODE_STATE NAME=MMU_POST_FORM_TIP_state

    # This is a good place to move to a position where oozing is not a problem
    # in the case of toolhead tip cutting where moving in the _MMU_PRE_UNLOAD
    # is too early
    {% if park_after_form_tip %}
        _MMU_PARK
    {% endif %}

    RESTORE_GCODE_STATE NAME=MMU_POST_FORM_TIP_state


###########################################################################
# This occurs immediately after unloading filament on a toolchange
#
[gcode_macro _MMU_POST_UNLOAD]
description: Optional post unload routine for filament change
gcode:
    SAVE_GCODE_STATE NAME=MMU_POST_UNLOAD_state

    # This is a good place to inject logic to, for example, perform tip cutting when
    # cutter is located at the MMU, thus preping the unloaded filment for next use

    RESTORE_GCODE_STATE NAME=MMU_POST_UNLOAD_state


###########################################################################
# This occurs prior to starting the load sequence on a toolchange
#
[gcode_macro _MMU_PRE_LOAD]
description: Optional pre load routine for filament change
gcode:
    SAVE_GCODE_STATE NAME=MMU_PRE_LOAD_state

    # We repeat the parking logic here so that the load sequence can run
    # independently from unload sequence
    _MMU_AUTO_HOME
    _MMU_SAVE_POSITION
    _MMU_PARK

    RESTORE_GCODE_STATE NAME=MMU_PRE_LOAD_state


###########################################################################
# This occurs after loading new filament on a toolchange
#
[gcode_macro _MMU_POST_LOAD]
description: Optional post load routine for filament change
gcode:
    SAVE_GCODE_STATE NAME=MMU_POST_LOAD_state

    # A good place to implement custom purging logic and/or nozzle cleaning
    # Also move back to original saved position in _MMU_PRE_UNLOAD/_MMU_POST_FORM_TIP
    # Doing it hear rather than leaving it to Happy Hare ensures control
    # of movement and speed
    _MMU_RESTORE_POSITION

    RESTORE_GCODE_STATE NAME=MMU_POST_LOAD_state


###########################################################################
#    ADVANCED   ADVANCED   ADVANCED   ADVANCED   ADVANCED   ADVANCED
# User modifable loading and unloading sequences
#
# By default Happy Hare will call internal logic to handle loading and unloading
# sequences. To enable the calling of user defined sequences you must add the
# following to your mmu_parameters.cfg
#
# gcode_load_sequence: 1	# Gcode loading sequence 1=enabled, 0=internal logic (default)
# gcode_unload_sequence: 1	# Gcode unloading sequence, 1=enabled, 0=internal logic (default)
#
# This reference example load sequence mimicks the internal ones exactly. It uses the
# high level "modular" movements that are all controlled by parameters defined in
# mmu_parameters.cfg and automatically keep the internal filament position state up-to-date.
# Switching to these macros should not change behavior and can serve as a starting point for
# your customizations
#
# State Machine:
# If you experiment beyond the basic example shown here you will need to understand
# the possible states for filament position.  This is the same state that is exposed
# as the `printer.mmu.filament_pos` printer variable. This internal state must be
# kept up-to-date and will need to be set directly as you progress through your
# custom move sequence.  At this time the state machine is non-extensible.
#
#        FILAMENT_POS_UNKNOWN = -1
#  L  ^  FILAMENT_POS_UNLOADED = 0
#  O  |  FILAMENT_POS_HOMED_GATE = 1     # If gate sensor fitted
#  A  |  FILAMENT_POS_START_BOWDEN = 2
#  D  |  FILAMENT_POS_IN_BOWDEN = 3
#        FILAMENT_POS_END_BOWDEN = 4
#  |  U  FILAMENT_POS_HOMED_ENTRY = 5    # If extruder (entry) sensor fitted
#  |  N  FILAMENT_POS_HOMED_EXTRUDER = 6
#  |  L  FILAMENT_POS_PAST_EXTRUDER = 7
#  |  O  FILAMENT_POS_HOMED_TS = 8       # If toolhead sensor fitted
#  |  A  FILAMENT_POS_IN_EXTRUDER = 9    # AKA Filament is past the Toolhead Sensor
#  v  D  FILAMENT_POS_LOADED = 10        # AKA Filament is homed to the nozzle
#
# Final notes:
# 1) You need to respect the context being passed into the macro such as the
#    desired 'length' to move because this can be called for test loading
# 2) The unload macro can be called with the filament in any position (states)
#    You are required to handle any starting point. The default reference
#    serves as a good guide
#
[gcode_macro _MMU_LOAD_SEQUENCE]
description: Called when MMU is asked to load filament
gcode:
    {% set filament_pos = params.FILAMENT_POS|float %}
    {% set length = params.LENGTH|float %}
    {% set full = params.FULL|int %}
    {% set home_extruder = params.HOME_EXTRUDER|int %}
    {% set skip_extruder = params.SKIP_EXTRUDER|int %}
    {% set extruder_only = params.EXTRUDER_ONLY|int %}

    {% if extruder_only %}
        _MMU_STEP_LOAD_TOOLHEAD EXTRUDER_ONLY=1

    {% elif filament_pos >= 7 %}                        # FILAMENT_POS_PAST_EXTRUDER
        {action_raise_error("Can't load - already in extruder!")}

    {% else %}
        {% if filament_pos <= 0 %}                      # FILAMENT_POS_UNLOADED
            _MMU_STEP_LOAD_GATE
        {% endif %}

        {% if filament_pos < 4 %}                       # FILAMENT_POS_END_BOWDEN
            _MMU_STEP_LOAD_BOWDEN LENGTH={length}
        {% endif %}

        {% if filament_pos < 6 and home_extruder %}     # FILAMENT_POS_HOMED_EXTRUDER
            _MMU_STEP_HOME_EXTRUDER
        {% endif %}

        {% if not skip_extruder %}                      # FILAMENT_POS_PAST_EXTRUDER
            _MMU_STEP_LOAD_TOOLHEAD
        {% endif %}

    {% endif %}

[gcode_macro _MMU_UNLOAD_SEQUENCE]
description: Called when MMU is asked to unload filament
gcode:
    {% set filament_pos = params.FILAMENT_POS|float %}
    {% set length = params.LENGTH|float %}
    {% set extruder_only = params.EXTRUDER_ONLY|int %}
    {% set park_pos = params.PARK_POS|float %}

    {% if extruder_only %}
        {% if filament_pos >= 7 %}                      # FILAMENT_POS_PAST_EXTRUDER
            _MMU_STEP_UNLOAD_TOOLHEAD EXTRUDER_ONLY=1 PARK_POS={park_pos}
        {% else %}
            {action_raise_error("Can't unload extruder - already unloaded!")}
        {% endif %}

    {% elif filament_pos == 0 %}
        {action_raise_error("Can't unload - already unloaded!")}

    {% else %}
        {% if filament_pos >= 7 %}                      # FILAMENT_POS_PAST_EXTRUDER
            # Exit extruder, fast unload of bowden, then slow unload encoder
            _MMU_STEP_UNLOAD_TOOLHEAD PARK_POS={park_pos}
        {% endif %}

        {% if filament_pos >= 4 %}                      # FILAMENT_POS_END_BOWDEN
            # Fast unload of bowden, then slow unload encoder
            _MMU_STEP_UNLOAD_BOWDEN FULL=1
            _MMU_STEP_UNLOAD_GATE

        {% elif filament_pos >= 2 %}                    # FILAMENT_POS_START_BOWDEN
            # Have to do slow unload because we don't know exactly where in the bowden we are
            _MMU_STEP_UNLOAD_GATE FULL=1
        {% endif %}

    {% endif %}

#
# Some examples of alternative macros follow
#
# 1. This loading example leverages the built-in modules to load filament to the end
# of the bowden tube. Then homes the filament to the toolhead sensor (mmu_toolhead)
# using synchronized gear and extruder movement. The state is updated to reflect this
# new position. It then performs a synchronized stepper move of 62mm to advance the
# filament to the nozzle
#
#[gcode_macro _MMU_LOAD_SEQUENCE]
#description: Called when MMU is asked to load filament
#gcode:
#    {% set filament_pos = params.FILAMENT_POS|float %}
#    {% set length = params.LENGTH|float %}
#    {% set skip_extruder = params.SKIP_EXTRUDER|int %}
#    {% set extruder_only = params.EXTRUDER_ONLY|int %}
#
#    {% if extruder_only %}
#        _MMU_STEP_HOMING_MOVE ENDSTOP=mmu_toolhead MOVE=50 MOTOR=extruder
#        _MMU_STEP_SET_FILAMENT STATE=8		# FILAMENT_POS_HOMED_TS
#        _MMU_STEP_MOVE MOVE=62 MOTOR=extruder
#        _MMU_STEP_SET_FILAMENT STATE=10	# FILAMENT_POS_LOADED
#    {% else %}
#        _MMU_STEP_LOAD_GATE
#        _MMU_STEP_LOAD_BOWDEN LENGTH={length}
#        {% if full and not skip_extruder %}
#            _MMU_STEP_HOMING_MOVE ENDSTOP=mmu_toolhead MOVE=50 MOTOR=gear+extruder
#            _MMU_STEP_SET_FILAMENT STATE=8	# FILAMENT_POS_HOMED_TS
#            _MMU_STEP_MOVE MOVE=62 MOTOR=gear+extruder
#            _MMU_STEP_SET_FILAMENT STATE=10	# FILAMENT_POS_LOADED
#        {% endif %}
#    {% endif %}
#
#
# 2. This very streamlined loading example starts off similarly loading to the end of the
# calibrated bowden. It then simply homes to the nozzle (using TMC stallguard on the extruder
# stepper!) with synchronized extruder+gear steppers.  This requires the `mmu_ext_touch`
# endstop to be one of those defined for the extruder stepper
#
#[gcode_macro _MMU_LOAD_SEQUENCE]
#description: Called when MMU is asked to load filament
#gcode:
#    {% set length = params.LENGTH|float %}
#    {% set full = params.FULL|int %}
#    {% set skip_extruder = params.SKIP_EXTRUDER|int %}
#    {% set extruder_only = params.EXTRUDER_ONLY|int %}
#
#    {% if extruder_only %}
#        _MMU_STEP_HOMING_MOVE ENDSTOP=mmu_ext_touch MOVE=100 MOTOR=extruder
#        _MMU_STEP_SET_FILAMENT STATE=10	# FILAMENT_POS_LOADED
#    {% else %}
#        _MMU_STEP_LOAD_GATE
#        _MMU_STEP_LOAD_BOWDEN LENGTH={length}
#        {% if full and not skip_extruder %}
#            _MMU_STEP_HOMING_MOVE ENDSTOP=mmu_ext_touch MOVE=100 MOTOR=extruder+gear
#            _MMU_STEP_SET_FILAMENT STATE=10	# FILAMENT_POS_LOADED
#        {% endif %}
#    {% endif %}

